#ifndef __FLIGHTDATA_TRANSFER__
#define __FLIGHTDATA_TRANSFER__

#include <stdint.h>

#include <string>
#include <vector>
#include <deque>

#include <base/osa/osa++.h>
#include <network/Socket++.h>


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

struct FlightData_Simp
{
public:
    enum {
        FD_MAX_CONTROLINPUT = 16,
        FD_MAX_ENGINES      = 8,
        FD_MAX_WHEELS       = 3,
        FD_MAX_TANKS        = 4,
    };

public:
    uint32_t    version;                            ///< protocol version

    // UAV information
    int32_t     uavType;                            ///< UAV type
                                                    ///<    0 - Generic micro air vehicle
                                                    ///<    1 - Fixed wing
                                                    ///<    2 - Quadcopter
                                                    ///<    4 - Helicopter
                                                    ///<    7 - Airship

    uint32_t    uavID;                              ///< UAV ID number

    double      timestamp;                          ///< timestamp of the message (seconds since 1970/1/1)

    // control input
    uint32_t    num_controlInput;                   ///< channel number of control input
    float       controlInput[FD_MAX_CONTROLINPUT];  ///< user control input (joystick input)
                                                    ///<    the value range is [-1, 1]
                                                    ///
                                                    ///     0       - roll
                                                    ///     1       - pitch
                                                    ///     2       - throttle
                                                    ///     3       - yaw
                                                    ///
    // position and attitude
    double      lat;                                ///< latitude
    double      lng;                                ///< longitude
    double      alt;                                ///< alititude
    double      H;                                  ///< hight above ground (start point)

    double      yaw, pitch, roll;                   ///< attitude (in degree)
    double      driftAngle;                         ///< drift angle to airway (in degree)

    // status and mode
    uint32_t    uavStatus;                          ///< current status
                                                    ///<    0 - disarmed, on ground
                                                    ///<    1 - normal, in flight
                                                    ///<    2 - take off
                                                    ///<    3 - landing
    uint32_t    flightMode;                         ///< flight mode
                                                    ///<    0 - stablize (manual control)
                                                    ///<    2 - alt_hold
                                                    ///<    3 - auto
                                                    ///<    4 - guided
                                                    ///<    5 - loiter
                                                    ///<    6 - RTL
                                                    ///<    9 - auto land

    // GPS
    float       gpsHDOP;                            ///< HDOP of current GPS
    uint32_t    gpsSatN;                            ///< number of satelliate can seen

    // Telemetry information
    float       telemRSSI;                          ///< Telemetry RSSI information
    float       videoRSSI;                          ///< Video RSSI information

    // Consumables
    uint32_t    num_tanks;                          ///< Max number of fuel tanks
    float       fuel_quantity[FD_MAX_TANKS];        ///< Fuel quantity
    float       battV;                              ///< main battery voltage

    // Engine status
    uint32_t    num_engines;                        ///< Number of valid engines
    uint32_t    eng_state[FD_MAX_ENGINES];          ///< Engine state (0:off, 1:cranking, 2:running)
    float       rpm[FD_MAX_ENGINES];                ///< Engine RPM rev/min


    // Control surface positions (normalized values)
    float       elevator;
    float       left_flap;
    float       right_flap;
    float       left_aileron;
    float       right_aileron;
    float       rudder;

    // land gear
    uint32_t    landgearState;                      ///< land gear state
                                                    ///<    0 - DEPLOYED, 1 - RETRACTED

    // environment
    float       airSpeed;                           ///< air speed
    float       groundSpeed;                        ///< ground speed
    float       tempture;                           ///< tempture

    // CRC
    uint8_t     crc[8];

public:
    FlightData_Simp() {
        uint8_t *p = (uint8_t*) this;
        memset(p, 0, sizeof(this));
    }

    int calcCRC(void) {
        uint8_t *p = (uint8_t*) this;

        int n = sizeof(this) - 8;
        int sum = 0;

        for(int i=0; i<n; i++) {
            sum += p[i];
        }

        sum = sum % 256;
        crc[0] = sum;

        return 0;
    }

    int checkCRC(void) {
        uint8_t *p = (uint8_t*) this;

        int n = sizeof(this) - 8;
        int sum = 0;

        for(int i=0; i<n; i++) {
            sum += p[i];
        }

        sum = sum % 256;
        if( sum != crc[0] ) return -1;
        else return 0;
    }

    int print(void) {
        printf("UAV[%3d] uavType = %d, uavStatus = %d, landgear = %d, protocol version: %d\n",
                        uavID, uavType, uavStatus, landgearState, version);
        printf("    lat, lng, alt, H  = %12f, %12f - %12f (%12f)\n",
                        lat, lng, alt, H);
        printf("    yaw, pitch, roll  = %12f %12f %12f, driftAngle = %12f\n",
                        yaw, pitch, roll, driftAngle);
        printf("    flightMode = %2d, HDOP = %4f, nSat = %2d, RSSI = %4f, timeStamp = %f\n",
                        flightMode, gpsHDOP, gpsSatN, telemRSSI, timestamp);
        printf("    battery = %6.2f, numTank = %d, fuel[0] = %6.1f, fuel[1] = %6.1f\n",
                        battV, num_tanks, fuel_quantity[0], fuel_quantity[1]);
        printf("    num_engines = %2d, rpm[0] = %6.1f, rpm[1] = %6.1f, rpm[2] = %6.1f, rpm[3] = %6.1f\n",
                        num_engines, rpm[0], rpm[1], rpm[2], rpm[3]);
        printf("    control: %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
                        controlInput[0], controlInput[1], controlInput[2], controlInput[3],
                        controlInput[4], controlInput[5], controlInput[6], controlInput[7]);
        printf("    E = %6.1f, F = %6.1f, %6.1f, A = %6.1f, %6.1f, R = %6.1f\n",
                        elevator, left_flap, right_flap, left_flap, right_flap, rudder);

        return 0;
    }
};

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

typedef std::vector<FlightData_Simp> FlightDataArray;

int saveFlightData(const std::string &fn, FlightDataArray &fda);
int loadFlightData(const std::string &fn, FlightDataArray &fda);


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class FlightData_Transfer : public pi::RThread
{
public:
    FlightData_Transfer();
    virtual ~FlightData_Transfer();

    virtual int thread_func(void *arg=NULL);

    int begin(const std::string &addr, int port);
    int stop(void);
    int isRunning(void) { return m_bOpened; }

    int sendPOS(FlightData_Simp *d);

private:
    pi::RSocket                 m_socket;
    int                         m_bOpened;

    std::deque<FlightData_Simp> m_dataQueue;
    pi::RMutex                  m_mutex;
};

#endif // end of __FLIGHTDATA_TRANSFER__
